#ifndef __SE3_GAUSSIAN_PDF_H
#define __SE3_GAUSSIAN_PDF_H

#include <TooN/TooN.h>
#include <TooN/se3.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>

class SE3GaussianPDF
{
public:
  // Default constructor  
 	SE3GaussianPDF();

  // Rotation and translation
  SE3GaussianPDF(const TooN::SE3<> &se3RT); // Assumes covariance matrix all zeros
  SE3GaussianPDF(const TooN::SE3<> &se3RT, const TooN::Matrix<6,6> &m66CovRT);
  SE3GaussianPDF(const TooN::SO3<> &so3R, const TooN::Vector<3> &v3CovR, const TooN::Vector<3> &v3T, const TooN::Vector<3> &v3CovT);
  SE3GaussianPDF(const TooN::SO3<> &so3R, const TooN::Vector<3> &v3T, const TooN::Matrix<6,6> &m66CovRT);

  // Rotation only
  SE3GaussianPDF(const TooN::SO3<> &so3R, const TooN::Vector<3> &v3CovR);
  SE3GaussianPDF(const TooN::SO3<> &so3R, const TooN::Matrix<3,3> &m33CovR);

  SE3GaussianPDF inverse();
  bool has_rotation() { return bHasRotation; };
  TooN::SO3<>& get_rotation() { return se3Mean.get_rotation(); };
  const TooN::SO3<>& get_rotation() const { return se3Mean.get_rotation(); }; 
  bool has_translation() { return bHasTranslation; };
  TooN::Vector<3>& get_translation() { return se3Mean.get_translation(); };
  const TooN::Vector<3>& get_translation() const { return se3Mean.get_translation(); };
  TooN::Matrix<6,6>& get_covariance() { return m66Covariance; }; 
  const TooN::Matrix<6,6>& get_covariance() const { return m66Covariance; }; 
  TooN::SE3<>& get_mean() { return se3Mean; };
  const TooN::SE3<>& get_mean() const { return se3Mean; }; 

  //MRPT style pose and inverse pose composition 
  void operator += (const SE3GaussianPDF &rhs);
  SE3GaussianPDF operator + (const SE3GaussianPDF &rhs) const;
  void operator -= (const SE3GaussianPDF &rhs);
  SE3GaussianPDF operator - (const SE3GaussianPDF &rhs) const;

private:
  TooN::SE3<> se3Mean;
  TooN::Matrix<6,6> m66Covariance; //x,y,z,r,p,y
  bool bHasRotation;
  bool bHasTranslation;
};

#endif

